#pragma once

#include "ecal/ecal.h"
#include "ecal/msg/protobuf/publisher.h"
#include "iostream"
#include "mutex"
#include "xict_common/utils/types.h"
#include "xict_cpp/bridge/udp_socket.h"
#include "xict_cpp/core/stage/base.h"
#include "xict_proto/excavator/js_state.pb.h"
#include "xict_proto/sensor/can.pb.h"

namespace xict_cpp {
    namespace core {
        namespace stage {
            struct CanetProcessorStageParams {
                /// @brief canet ip
                std::string canet_ip = "127.0.0.1";
                /// @brief canet 端口号
                int canet_port = 7000;
                /// @brief canet 厂家
                std::string canet_factory = "zlg";
                /// @brief 远程车辆的型号
                std::string vehicle_type = "";
                /// @brief 是否使用体感平台
                bool have_sensation_platform = true;
                /// @brief 体感平台ip
                std::string sensation_platform_ip = "192.168.1.88";
                /// @brief 体感平台端口号
                int sensation_platform_port = 8080;
                /// @brief 体感平台x轴运动范围
                int sensation_platform_range_x = 7;
                /// @brief 体感平台y轴运动范围
                int sensation_platform_range_y = 7;
                /// @brief 体感平台z轴运动范围
                int sensation_platform_range_z = 7;
                /// @brief 是否使用自制手柄
                bool use_selfmade_handle = false;
                /// @brief can转eCAL消息发布帧率
                int msg_framerate = 30;
            };

            /// @brief can帧数据结构
            struct can_msgs {
                uint8_t dlc;
                uint32_t id;
                uint8_t data[8];
                bool is_rtr;
                bool is_extended;
            };

            struct CanetProcessorStagePulisher {
#ifdef SELFMADE_HANDLE
                /// @brief 右操作杆状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::JoystickStatus>
                    right_joystick_status_pub;
                /// @brief 左操作杆状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::JoystickStatus>
                    left_joystick_status_pub;
                /// @brief 右手柄状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::HandleStatus>
                    right_handle_status_pub;
                /// @brief 左手柄状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::HandleStatus>
                    left_handle_status_pub;
#endif

#ifdef GRADER
                /// @brief 平地机状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::Grader_CAN>
                    grader_can_status_pub;
#endif

#ifdef LOADER
                /// @brief 装载机状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::Loader_CAN>
                    loader_can_status_pub;
#endif

#ifdef BACKHOE_LOADER
                /// @brief 压路机状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::BackhoeLoader_CAN>
                    backhoe_loader_can_status_pub;
#endif

#ifdef SCRAPER
                /// @brief 铲运机状态数据发布器
                eCAL::protobuf::CPublisher<xcmg_proto::Scraper_CAN>
                    scraper_can_status_pub;
#endif
            };

            struct CanetProcessorStageData {
                /// @brief 右操作杆状态数据指针
                std::shared_ptr<xcmg_proto::JoystickStatus>
                    right_joystick_status_ptr;
                /// @brief 左操作杆状态数据指针
                std::shared_ptr<xcmg_proto::JoystickStatus>
                    left_joystick_status_ptr;
                /// @brief 右手柄状态数据指针
                std::shared_ptr<xcmg_proto::HandleStatus>
                    right_handle_status_ptr;
                /// @brief 左手柄状态数据指针
                std::shared_ptr<xcmg_proto::HandleStatus>
                    left_handle_status_ptr;

                /// @brief 平地机状态数据指针
                std::shared_ptr<xcmg_proto::Grader_CAN> grader_can_status_ptr;
                /// @brief 装载机状态数据指针
                std::shared_ptr<xcmg_proto::Loader_CAN> loader_can_status_ptr;
                /// @brief 压路机状态数据指针
                std::shared_ptr<xcmg_proto::BackhoeLoader_CAN>
                    backhoe_loader_can_status_ptr;
                /// @brief 铲运机状态数据指针
                std::shared_ptr<xcmg_proto::Scraper_CAN> scraper_can_status_ptr;
            };

            class CanetProcessorStage : public Stage {
            public:
                /// @brief 默认构造函数
                CanetProcessorStage() = default;
                /// @brief 默认析构函数
                ~CanetProcessorStage() override = default;
                /// @brief 初始化函数
                bool Init() override;
                /// @brief 初始化函数
                bool Init(const CanetProcessorStageParams& params);
                /// @brief 运行函数
                bool Process() override;
                /// @brief 返回类的名称
                std::string Name() const override;

            protected:
                /// @brief 初始化，并返回结果
                bool Initialize() override;
                /// @brief 重置函数
                void Reset() override;

            public:
                /**
                 * @brief 是否是空数据
                 *
                 * @param can_ptr can帧
                 * @return true
                 * @return false
                 */
                bool IsAllZeroValues(const can_msgs* can_ptr);
                /**
                 * @brief can数据转成udp发布
                 *
                 * @param can_msg can帧
                 */
                void ConvertCanToUdp(const can_msgs can_msg);
                /**
                 * @brief udp数据转成can数据
                 *
                 * @param udp_ptr udp数据
                 * @param can_ptr can数据
                 */
                void ConvertUdpToCan(const uint8_t* udp_ptr, can_msgs* can_ptr);
                /**
                 * @brief 处理can数据
                 *
                 * @param data 数据
                 * @param size 数据长度
                 */
                void OnMessage(const void* data, size_t size);
                /**
                 * @brief 向体感平台发送IMU数据
                 *
                 * @param euler_x
                 * @param euler_y
                 * @param euler_z
                 * @param acc_raw_x
                 * @param acc_raw_y
                 * @param acc_raw_z
                 */
                void SendImuData(float euler_x, float euler_y, float euler_z,
                                 float acc_raw_x, float acc_raw_y,
                                 float acc_raw_z);
                /**
                 * @brief 解析can帧
                 *
                 * @param c can帧数据
                 */
                void ParseCanData(const can_msgs* c);
                /**
                 * @brief 解析左手柄数据
                 *
                 * @param c can帧数据
                 */
                void ParseLeftHandleData(const can_msgs* c);
                /**
                 * @brief 解析右手柄数据
                 *
                 * @param c can帧数据
                 */
                void ParseRightHandleData(const can_msgs* c);

            private:
                std::shared_ptr<bridge::UdpSocket> udp_sock_ptr_;
                std::shared_ptr<bridge::UdpSocket> imu_sock_ptr_;
                boost::asio::io_service io_service_;
                /// @brief imu数据中累加值
                uint32_t t_cur_ = 0;
                /// @brief 互斥锁
                std::timed_mutex mtx_;
                float euler_x_ = 0.0, euler_y_ = 0.0, euler_z_ = 0.0;
                float acc_raw_x_ = 0.0, acc_raw_y_ = 0.0, acc_raw_z_ = 0.0;
                /// @brief CanetProcessorStageParams参数
                CanetProcessorStageParams params_;
                /// @brief ecal消息发布器
                CanetProcessorStagePulisher pub_;
                /// @brief ecal数据消息
                CanetProcessorStageData data_;
                xict_common::utils::Int16_bytes tmp_int16_;
            };
        }   // namespace stage
    }       // namespace core
}   // namespace xict_cpp